/****************************************************************************\
 * @file     BR_ServoMotor.c
 * @author   傅帅
 * @date     2019/12/30
 * @brief    舵向电机
 * @note     与C6120驱动板交互，维护舵向电机状态，提供控制接口
 ******************************************************************************
 * @attent   此文件除了接近开关引脚变动外一般不用改动，若需改动此文件，请先了解C6120驱动板工作原理
 *           四个电机默认顺序为[右前、右后、左前、左后]. ID号默认为0,1,2,3
 ****************************************************************************/
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "string.h"
#include "arm_math.h"
#include "BR_Common.h"

/* 全局变量 */
TaskHandle_t BR_ServoMotorHandlerTaskHandler;
QueueHandle_t BR_ServoMotorQueue;
/* 静态变量 */
static float prvReduRatio = BR_configServoReductionRatio;
static uint8_t prvTxBuffer[SERVO_MOTOR_TX_MAX_LEN], prvRxBuffer[SERVO_MOTOR_RX_MAX_LEN];    // 接收和发送的字符缓冲区
static int16_t prvZeroCounts[4] = BR_configSERVO_MOTORS_ZERO_COUNT;    // 四个电机处于零位时的计数(驱动器提供的绝对count)
static int32_t prvAbsCounts[4];                                        // 当前相对于逻辑零点的绝对count
static uint8_t prvInitACK_Flag, prvZeroACK_Flag;                       // 用于标志初始化和归零操作是否接收到了应答消息
static void prvSetLogicZeroPos(uint8_t MotorId);
static void prvOnServoMotorRecv(uint8_t *pData, uint32_t Len);
/* -------------------------------------------------- Public ----------------------------------------------- */
/** @brief 舵向电机接收任务本体，从队列中拿取消息长度，从缓冲区读取消息，不断更新舵向电机的实时状态 **/
void BR_vServoMotorHandlerTask(void)
{
    uint32_t RxLen;
    uint8_t Sum, *pMsg;          // 局部指针用于消息处理
    while(1)
    {
        xQueueReceive(BR_ServoMotorQueue, &RxLen, portMAX_DELAY);
        pMsg = prvRxBuffer;
        if(*pMsg != 'A') continue;  // 第一个字节若非attention，等下一帧
        Sum = 0;
        for(; pMsg < prvRxBuffer + RxLen - 1; pMsg++) Sum += *pMsg; // 到校验位前全加起来
        if((uint8_t)Sum == *pMsg)   // 若校验通过
        {
            pMsg = prvRxBuffer + 1;         // 重新回到A后一字节
            switch(*pMsg++)
            {
                case 0x00:                  // 对于正常绝对计数消息的处理
                {
                    uint8_t MotorId = *pMsg++;              // 拿取MotorId
                    if(MotorId > 3)break;                   // 默认舵向电机在驱动板中的ID号为0-3
                    int32_t AbsCount = *(int32_t *)pMsg;    // 拿取当前绝对计数
                    prvAbsCounts[MotorId] = AbsCount;       // 更新绝对计数
                    /* 相对位置计算 */
                    float AbsPos = (float)AbsCount * 2 * PI / ( 8192 * BR_configServoReductionRatio );  // 计算当前相对位置,更新全局变量
                    AbsPos = fmod(AbsPos,2 * PI);           // 对2PI取余，得到相对位置
                    if(AbsPos > PI) AbsPos -= 2 * PI;       // 范围限制在-PI到PI
                    else if(AbsPos < -PI) AbsPos += 2 * PI;
                    BR_gRobot.ServoMotors.RelativePos[MotorId] = AbsPos;
                    break;
                }
                case 0x01:                  // 接收到初始化帧应答
                    prvInitACK_Flag = 1;
                    break;
                case 0x03:                  // 接收到归零帧应答
                    prvZeroACK_Flag = 1;
                    break;                        
            }                          
        }
    }
}

/** @brief 舵向电机相关外设初始化 **/
void BR_vServoMotorInit(void)
{
    BR_vUsartInitWithDMA(   BR_configSERVO_MOTOR_USART, SERVO_MOTOR_BAUDRATE,
                            SERVO_MOTOR_TX_MAX_LEN, SERVO_MOTOR_TX_PRIO,
                            SERVO_MOTOR_RX_MAX_LEN, SERVO_MOTOR_RX_PRIO, prvOnServoMotorRecv);
    BR_vCLoseSwitchGPIO_Init();
}

/** @brief 舵向电机向驱动板发送初始化信息 **/
void BR_vServoMotorInitSend(float VelP, float VelI, float VelD, float CurrentI_Limit, float Current_Limit,
                            float PosP, float PosI, float PosD, float VelI_Limit,     float Vel_Limit)
{
    for(int i = 0; i < 4; i++)  // 整理发送四个电机的初始化帧
    {
        int MotorId = i;        // 默认ID为0-3
        int timeout = 0;
        uint8_t TxLen = BR_cMemCat( prvTxBuffer, sizeof( prvTxBuffer ), "A%c%c%h%d%d%f%f%f%f%f%f%f%f%f%f", 0x01, MotorId, 
                                    prvZeroCounts[i], -BR_configMAX_SAFE_COUNTS, BR_configMAX_SAFE_COUNTS,
                                    VelP, VelI, VelD, CurrentI_Limit, Current_Limit,
                                    PosP, PosI, PosD, VelI_Limit,     Vel_Limit);
        BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);                                   // 校验位计算并加入
        prvInitACK_Flag = 0;        // 清零应答标志        
        while(prvInitACK_Flag == 0) // 直到收到应答标志前,持续发送
        {            
            BR_iSend(BR_configSERVO_MOTOR_USART, prvTxBuffer, TxLen + 1);
            vTaskDelay(1);
            timeout++;
            if(timeout>250)break;   // 单个舵机连接超时时间0.25s
        }
    }
    BR_ServoMotorQueue = xQueueCreate(1,4);    // Len=1 Size=4Byte    传当前接收帧长度
}

/** @brief 舵向电机位置设置,根据当前绝对计数将相对位置转换成下发的绝对计数
 *  @param Poses: 舵向相对位置的 位置数组 -PI to PI **/
void BR_vServoMotorsPosSet(float *Poses)
{
    for(int i = 0; i < 4; i++)
    {
        int MotorId = i;
        /* 相对位置Poses到绝对Counts的转换 */
        int32_t RelativeCounts = Poses[i] / PI * 4096 * prvReduRatio;   // 得到相对位置对应的相对Counts
        int32_t AbsoluteCounts = prvAbsCounts[i];                       // 得到绝对Counts
        int32_t Circle = AbsoluteCounts / ( 8192 * prvReduRatio );      // Circle表示当前绝对计数多转的圈数
        RelativeCounts += Circle * ( 8192 * prvReduRatio );             // 相对Counts加到和绝对同圈
        /* 经过此逻辑后的RelativeCounts就是最后需要的下发的绝对计数 */
        if(AbsoluteCounts - RelativeCounts > 4096 * prvReduRatio) RelativeCounts += 8192 * prvReduRatio;
        else if(AbsoluteCounts - RelativeCounts < -4096 * prvReduRatio) RelativeCounts -= 8192 * prvReduRatio;
        /* 发送绝对计数 */
        uint8_t TxLen = BR_cMemCat(prvTxBuffer, sizeof( prvTxBuffer ), "A%c%cp%d", 0x02, MotorId, RelativeCounts);
        BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);                                   // 校验位计算并加入
        while(BR_iSend(BR_configSERVO_MOTOR_USART, prvTxBuffer, TxLen + 1)){vTaskDelay(1);} // 发送        
    }
}

/** @brief 舵向电机速度设置
 *  @param Vels: 速度数组 counts/s **/
void BR_vServoMotorsVelSet(int16_t *Vels)
{
    for(int i = 0; i < 4; i++)
    {
        int MotorId = i;
        uint8_t TxLen = BR_cMemCat(prvTxBuffer, sizeof( prvTxBuffer ), "A%c%cv%h", 0x02, MotorId, Vels[i]);
        BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);                                   // 校验位计算并加入
        while(BR_iSend(BR_configSERVO_MOTOR_USART, prvTxBuffer, TxLen + 1)){vTaskDelay(1);} // 发送        
    }
}

/** @brief 舵向电机电流设置
 *  @param Vels: 电流数组 对于C610: -10000~10000 表示 -10~10A
                          对于C620: -16384~16384 表示 -20~20A **/
void BR_vServoMotorsCurSet(int16_t *Curs)
{
    for(int i = 0; i < 4; i++)
    {
        int MotorId = i;
        uint8_t TxLen = BR_cMemCat(prvTxBuffer, sizeof( prvTxBuffer ), "A%c%cc%h", 0x02, MotorId, Curs[i]);
        BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);                                   // 校验位计算并加入
        while(BR_iSend(BR_configSERVO_MOTOR_USART, prvTxBuffer, TxLen + 1)){vTaskDelay(1);} // 发送
    }
}

/** @brief 舵向电机校准,该函数将调用任务阻塞至四个电机全部校准结束
 *  @prarm CalVel: 校准时的速度 counts/s **/
void BR_vServoMotorCalibration(uint16_t CalVel)
{
    uint8_t CalFinFlag[4] = {0, 0, 0, 0};
    int16_t MotorsVel[4] = {0, 0, 0, 0};
    /* 找接近开关触发位 */
    while(CalFinFlag[0] + CalFinFlag[1] + CalFinFlag[2] + CalFinFlag[3] != 4)     // 全部校准结束后退出
    {
        for(int i = 0; i < 4; i++)
        {
            if(GPIO_ReadInputDataBit(GPIOE, 1 << ( 3 - i )) == 0)   // 若对应接近开关触发(接近开关接G8-G11) (2020.1.14 改为E3-E0)
            {
                CalFinFlag[i] = 1;
                MotorsVel[i] = 0;
            }
            if(CalFinFlag[i] == 0)  // 该电机未到位
                MotorsVel[i] = CalVel;
        }
        BR_vServoMotorsVelSet(MotorsVel);
        vTaskDelay(5);
    }
    /* 全部结束后, 发送设置逻辑零位指令 */
    for(int i = 0; i < 4; i++)
    {
        prvSetLogicZeroPos(i);
    }
}

/** @brief 舵向电机回逻辑零位,该函数将调用任务阻塞至四个电机全部回归逻辑零位 **/
void BR_vServoMotorsGoLogicZeroPos(void)
{
    for(int i = 0; i < 4; i++)
    {
        int MotorId = i;
        uint8_t TxLen = BR_cMemCat(prvTxBuffer, sizeof( prvTxBuffer ), "A%c%cp%d", 0x02, MotorId, 0);
        BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);                                       // 校验位计算并加入
        while(BR_iSend(BR_configSERVO_MOTOR_USART, prvTxBuffer, TxLen + 1)) {vTaskDelay(1);}    // 发送
    }
    while(  ABS(prvAbsCounts[0]) >  GO_LOGICZERO_WINDOW_COUNTS &&
            ABS(prvAbsCounts[1]) >  GO_LOGICZERO_WINDOW_COUNTS &&
            ABS(prvAbsCounts[2]) >  GO_LOGICZERO_WINDOW_COUNTS &&
            ABS(prvAbsCounts[3]) >  GO_LOGICZERO_WINDOW_COUNTS ){ vTaskDelay(5); } // 阻塞
}

/** @brief 舵向电机回离逻辑零位(不绞线位置)最近的Y轴(初始化时的正前方)
*          与回零位类似，但会回到轮子与Y轴平行的状态，而不是与车体平行且同向 **/
void BR_vServoMotorsGoLogicZeroRobotThetaPos(void)
{
    int TargetCounts;   // 当前离逻辑零位最近的Y轴位置对应的的绝对计数
    float TmpTheta;     // TmpTheta为机器人车身Theta映射到-PI/2 TO PI/2的范围内
    
    if(BR_gRobot.ChassisState.Theta > PI/2)
        TmpTheta = BR_gRobot.ChassisState.Theta - PI;
    else if(BR_gRobot.ChassisState.Theta < -PI/2)
        TmpTheta = BR_gRobot.ChassisState.Theta + PI;
    else 
        TmpTheta = BR_gRobot.ChassisState.Theta;
    TargetCounts = -TmpTheta / PI * 4096 * BR_configServoReductionRatio;
    
    for(int i = 0; i < 4; i++)
    {
        uint8_t TxLen = BR_cMemCat(prvTxBuffer, sizeof( prvTxBuffer ), "A%c%cp%d", 0x02, i, TargetCounts);
        BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);               // 校验位计算并加入
        while(BR_iSend(BR_configSERVO_MOTOR_USART, prvTxBuffer, TxLen + 1)) {vTaskDelay(1);}     // 下发当前离逻辑零位最近的Y轴位置对应的的绝对计数
    }
    while(  ABS(prvAbsCounts[0] - TargetCounts) >  GO_LOGICZERO_WINDOW_COUNTS &&
            ABS(prvAbsCounts[1] - TargetCounts) >  GO_LOGICZERO_WINDOW_COUNTS &&
            ABS(prvAbsCounts[2] - TargetCounts) >  GO_LOGICZERO_WINDOW_COUNTS &&
            ABS(prvAbsCounts[3] - TargetCounts) >  GO_LOGICZERO_WINDOW_COUNTS ){ vTaskDelay(5); } // 阻塞至四轮均进入窗口
}

/** @brief 接近开关相关GPIO初始化 **/
void BR_vCLoseSwitchGPIO_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;
    RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOE, ENABLE);// 使能GPIOE时钟
    GPIO_InitStructure.GPIO_Pin = BR_configCLOSE_SWITCH_START_GPIO      |
                                  BR_configCLOSE_SWITCH_START_GPIO << 1 |
                                  BR_configCLOSE_SWITCH_START_GPIO << 2 |
                                  BR_configCLOSE_SWITCH_START_GPIO << 3;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;        // 输入模式
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;        // 上拉输入
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;  // 100MHz
    GPIO_Init(GPIOE, &GPIO_InitStructure);              // 初始化GPIO
}

/* -------------------------------------------------- Private ----------------------------------------------- */
/** @brief 舵向电机对应串口接收回调 **/
static void prvOnServoMotorRecv(uint8_t *pData, uint32_t Len)
{
    if(BR_ServoMotorQueue == NULL)  /* 任务正常调度前，在此处接受InitACK */
    {
        uint8_t *pMsg = pData;
        if(*pMsg != 'A') return;    // 第一个字节若非attention，等下一帧
        int Sum = 0;
        for(; pMsg < pData + Len - 1; pMsg++) Sum += *pMsg; // 到校验位前全加起来
        if((uint8_t)Sum == *pMsg)   // 若校验通过
        {
            pMsg = pData + 1;       // 重新回到A后一字节
            switch(*pMsg++)
            {
                case 0x01:          // 接收到初始化帧应答
                    prvInitACK_Flag = 1;
                    break;
            }                          
        }
    }else{                          /* 任务正常调度后，将消息发往任务 */
        BaseType_t IfWoken;
        memcpy(prvRxBuffer, pData, Len);                        // 防止Usart中的缓冲区被更改，先复制到自己的缓冲区
        xQueueSendFromISR(BR_ServoMotorQueue, &Len, &IfWoken);  // 向任务发送通知
    }   
}

/** @brief 单个电机设置逻辑零位(将累计圈数归零，当前圈内的ZeroCounts作为绝对零位) **/
static void prvSetLogicZeroPos(uint8_t MotorId)
{
    uint8_t TxLen = BR_cMemCat(prvTxBuffer, sizeof( prvTxBuffer ), "A%c%c", 0x03, MotorId);
    BR_vCheckByteCalculateAppend(prvTxBuffer, TxLen);           // 校验位计算并加入
    prvZeroACK_Flag = 0;                                        // 清零应答标志
    while(prvZeroACK_Flag == 0)                                 // 直到收到应答标志前,持续发送
    {
        BR_iSend(BR_configSERVO_MOTOR_USART, prvTxBuffer, TxLen + 1);
        vTaskDelay(1);
    }
}
